#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# By yongcong.wang @ 13/10/2020

import math
import numpy as np

def cv_z(x0, dx, y0, dy, dt, cnt):
    Z = [np.array([
            [x0],
            [dx],
            [y0],
            [dy]
        ])]
    for i in np.arange(1, cnt):
        Z.append(np.array([
            [Z[i-1][0, 0] + dx * dt],
            [dx],
            [Z[i-1][2, 0] + dy * dt],
            [dy]
            ]))

    return Z

def ca_z(x0, dx, ddx, y0, dy, ddy, dt, cnt):
    Z = [np.array([
            [x0],
            [dx],
            [y0],
            [dy]
        ])]
    for i in np.arange(1, cnt):
        Z.append(np.array([
            [Z[i-1][0,0] + Z[i-1][1,0] * dt + 0.5 * ddx * dt**2],
            [Z[i-1][1,0]+ ddx * dt],
            [Z[i-1][2,0] + Z[i-1][3,0] * dt + 0.5 * ddy * dt**2],
            [Z[i-1][3,0]+ ddy * dt]
            ]))

    return Z

def ct_z(x0, dx, y0, dy, dtheta, dt, cnt):
    Z = [np.array([
            [x0],
            [dx],
            [y0],
            [dy]
        ])]
    theta = math.atan2(dy, dx)
    v = math.hypot(dx, dy)
    for i in np.arange(1, cnt):
        theta += dtheta * dt
        Z.append(np.array([
            [Z[i-1][0, 0] + v * dt * math.cos(theta)],
            [v * math.cos(theta)],
            [Z[i-1][2, 0] + v * dt * math.sin(theta)],
            [v * math.sin(theta)]
            ]))

    return Z

def add_noise(Z, gain):
    return [np.copy(z) + gain * (np.random.random(z.shape) - 0.5) for z in Z]

def veh_z():
    veh_x = [366663.97602476494, 366662.9946207391, 366662.6759009701, 366661.1622944487, 366659.58930389007, 366657.6565741472, 366656.4828280753, 366656.15361394116, 366654.88519447984, 366653.76224957936, 366653.1054994988, 366651.7114348413, 366651.16369055305, 366650.874347754, 366650.0616439397, 366649.3777791009, 366649.03453845903, 366648.4280717088, 366647.5943679608, 366647.2221279375, 366646.8465561758, 366646.5085824456, 366646.1340383063, 366644.3427743707, 366643.5881165787, 366642.846131407, 366642.45244532847, 366641.6908466484, 366641.3021880112, 366640.02856995794, 366639.4418653023, 366638.20045708644, 366637.2715907917, 366635.7736804261, 366634.0023193349, 366632.9302085623, 366632.56700302905, 366631.17368761724, 366630.0236935694, 366628.5333584803, 366627.37944804167, 366625.0030052651, 366623.1318177335, 366621.37524976314, 366621.06438229943, 366620.2211088776, 366619.4377327408, 366618.9085942329, 366618.006810715, 366616.7322307855, 366614.46118184115, 366612.7030734222, 366611.06044642493, 366610.19472178846, 366609.39105654863, 366606.6090175863, 366604.3080086172, 366598.9060458723, 366597.7777024018, 366596.7195034872, 366595.4905359966, 366593.75419501954, 366591.76287830947, 366590.9316969246, 366588.9129293184, 366586.7517641183, 366586.503739217, 366585.06951231934, 366582.5079019981, 366581.6616934893, 366580.8332130943, 366579.0708624139, 366579.0822817867, 366574.04615401925, 366572.23182608327, 366569.8904884309, 366569.20304190996, 366566.1490964378, 366564.1110583323, 366561.0055656462, 366557.7588090418]
    veh_y = [3452794.722778085, 3452794.7793918317, 3452794.7846858776, 3452794.8425639765, 3452794.859859606, 3452794.943143552, 3452795.01383029, 3452795.0187155725, 3452795.0543980645, 3452795.056762374, 3452795.105838403, 3452795.1812158637, 3452795.1416444327, 3452795.2034430234, 3452795.199478978, 3452795.2230842337, 3452795.228185663, 3452795.222976726, 3452795.4319262994, 3452795.392519834, 3452795.451516682, 3452795.2734702644, 3452795.403336747, 3452795.355112594, 3452795.3687331784, 3452795.271425586, 3452795.2614913397, 3452795.4582046536, 3452795.4890135247, 3452795.503599826, 3452795.5383209675, 3452795.412151686, 3452795.482762867, 3452795.4739192803, 3452795.3478328297, 3452795.4267890877, 3452795.392277105, 3452795.2418531007, 3452795.320684593, 3452795.118571157, 3452795.0164180165, 3452794.931638216, 3452794.7338928366, 3452794.719611816, 3452794.731344927, 3452794.7156484094, 3452794.6497643963, 3452794.66159596, 3452794.6561888997, 3452794.6781059774, 3452794.808928909, 3452794.754951435, 3452794.7838909705, 3452795.00084411, 3452795.062406074, 3452795.4179719803, 3452795.6870684233, 3452796.0814652485, 3452796.257860994, 3452796.1570433574, 3452796.2587265177, 3452796.341382583, 3452796.3802440464, 3452796.3916408345, 3452796.443479321, 3452796.5079668774, 3452796.485522179, 3452796.4657549523, 3452796.637628183, 3452796.514723928, 3452796.569685999, 3452796.716624091, 3452796.5667564655, 3452796.5611726535, 3452796.6226344635, 3452796.597070849, 3452796.6642564, 3452796.705468187, 3452796.6754435212, 3452796.777854291, 3452796.788157245]
    veh_vx = [-2.0457301139831543, -1.6275277137756348, -1.481507420539856, -1.5721306800842285, -1.5913820266723633, -1.711446762084961, -1.9512752294540405, -1.9528429508209229, -2.223374843597412, -2.3085055351257324, -2.4777731895446777, -2.50956654548645, -2.6480636596679688, -2.973581552505493, -2.904118061065674, -2.7575762271881104, -2.948342800140381, -2.877401828765869, -2.8440134525299072, -3.0466771125793457, -2.7784242630004883, -3.068000316619873, -2.8528850078582764, -3.2648470401763916, -3.2343382835388184, -3.480713129043579, -3.6524412631988525, -3.755077838897705, -3.7810585498809814, -3.990694046020508, -4.023892879486084, -4.242422580718994, -4.376273155212402, -4.49798059463501, -4.870119571685791, -5.108942031860352, -5.322845935821533, -5.2171478271484375, -5.0520734786987305, -5.521345615386963, -5.024937629699707, -4.704761981964111, -4.441832542419434, -4.153748989105225, -3.8849973678588867, -3.9184117317199707, -3.839837074279785, -3.8297390937805176, -3.7738473415374756, -3.8142247200012207, -3.9582109451293945, -3.847064971923828, -3.780780076980591, -3.576749801635742, -3.777549982070923, -4.601053714752197, -4.988664150238037, -5.534853458404541, -5.820122718811035, -5.744226932525635, -5.778326034545898, -5.627255916595459, -5.893857002258301, -5.749251365661621, -5.787508964538574, -5.773934364318848, -5.836709499359131, -5.809682846069336, -6.123110771179199, -6.851296901702881, -6.6507415771484375, -6.651390075683594, -6.995279312133789, -7.601310729980469, -7.775839328765869, -7.883184909820557, -7.9863176345825195, -7.968554496765137, -8.232288360595703, -8.069657325744629, -8.382157325744629]
    veh_vy = [0.03213828057050705, 0.059689901769161224, 0.035077374428510666, 0.04563223570585251, -0.0016576583730056882, 0.024200748652219772, 0.059037480503320694, 0.02201271429657936, 0.10545993596315384, 0.11560820788145065, 0.0005272969137877226, -0.028616689145565033, -0.0023022545501589775, -0.021318262442946434, -0.04088643565773964, 0.07851170003414154, 0.05645432323217392, 0.0007357442518696189, 0.045978743582963943, 0.1062195673584938, 0.13143151998519897, 0.1833164542913437, 0.0968942791223526, 0.0763341411948204, 0.048504866659641266, -0.13284415006637573, -0.11469078809022903, 0.15059320628643036, 0.24191589653491974, 0.20724499225616455, 0.2838839292526245, 0.16337160766124725, 0.19383645057678223, -0.034293510019779205, -0.23361560702323914, -0.21607182919979095, -0.22777225077152252, -0.32631608843803406, -0.45699799060821533, -0.31210339069366455, -0.23902200162410736, -0.08350783586502075, -0.10640449076890945, -0.052984755486249924, 0.03312419354915619, 0.19803743064403534, 0.3585452735424042, 0.3592667877674103, 0.38253459334373474, 0.24462901055812836, 0.23259013891220093, 0.24920426309108734, 0.3197888135910034, 0.33777230978012085, 0.3358265459537506, 0.5403535962104797, 0.4058835804462433, 0.27981337904930115, 0.4426557719707489, 0.39783811569213867, 0.49450671672821045, 0.4396500289440155, 0.273470014333725, 0.10902631282806396, 0.06052320823073387, 0.19513508677482605, 0.21698692440986633, 0.18330241739749908, 0.3825306296348572, 0.8777614235877991, 0.5815287828445435, 0.5906145572662354, 0.33125585317611694, 0.7801445126533508, -0.1767507791519165, 0.07661654055118561, 0.2465013712644577, 0.26706191897392273, 0.4562828540802002, 0.1739216446876526, 0.2454816997051239]

    z = []
    for i in range(len(veh_x)):
        z.append(np.array([
            [veh_x[i]],
            [veh_vx[i]],
            [veh_y[i]],
            [veh_vy[i]]
        ]))
    return z

def ped_z():
    ped_x = [366583.7586207068, 366584.0128932324, 366584.56668470363, 366584.8965274246, 366584.96823871817, 366585.10149136325, 366585.2266491947, 366585.8902639438, 366586.08900118154, 366586.21407185856, 366586.36237363383, 366586.99014688557, 366587.1842773995, 366587.2836528685, 366587.4213557554, 366587.61235941236, 366587.6200707757, 366587.7171604042, 366587.83296858775, 366588.446648015, 366588.4501254802, 366588.55877887533, 366588.7504491969, 366588.97016307304, 366589.2312950094, 366589.27575698216, 366589.32829185756, 366589.90464578057, 366590.144232042, 366590.24586949614, 366590.3177384324, 366590.40835125186, 366590.4665030377, 366590.49541966, 366590.64131664846, 366590.9943414579, 366591.1525334037, 366591.38564666064, 366591.53952350846, 366591.4116780433, 366591.63099214504, 366591.5618100828, 366591.7017052815, 366592.06871697406, 366592.1239869958, 366592.33838332817, 366592.6706793031, 366592.6894883165, 366592.9176207316, 366593.02116356516, 366593.43264232314, 366593.52811882726, 366593.50043680664, 366593.4953036112, 366593.7443676923, 366593.7523135081, 366593.9357344086, 366593.91548581555, 366594.1661268653, 366593.9963479345, 366594.12665532186, 366594.0540274834, 366594.1238355222, 366594.26799606916, 366594.2401390067, 366594.2774107874, 366594.3241178746, 366594.3921234738, 366594.39751701383, 366594.44049976225, 366594.42507136706, 366594.39357081545, 366594.5079210908]
    ped_y = [3452802.23274165, 3452802.2427489734, 3452802.157380052, 3452801.9229381387, 3452801.890366522, 3452801.943989553, 3452801.8828531355, 3452801.7501186607, 3452801.8306867997, 3452801.77883276, 3452801.752584325, 3452801.68809742, 3452801.586407931, 3452801.5152029754, 3452801.5195512967, 3452801.536490657, 3452801.4461812703, 3452801.506080153, 3452801.3508779416, 3452801.290994734, 3452801.290480953, 3452801.2281541023, 3452801.2339063124, 3452801.1602182873, 3452801.1165534845, 3452801.060248124, 3452801.028314167, 3452800.9844999015, 3452800.903987874, 3452800.8378190976, 3452800.9210015624, 3452800.899311129, 3452800.8318012473, 3452800.88191144, 3452800.874475844, 3452800.6752950405, 3452800.3824638696, 3452800.290975943, 3452800.278803653, 3452800.528730679, 3452800.1066831076, 3452799.981034644, 3452799.9040191146, 3452799.5212299614, 3452799.504520814, 3452799.1069297534, 3452798.6167423273, 3452798.6224304806, 3452797.942536528, 3452797.845152785, 3452797.171482465, 3452797.102554293, 3452796.9712729487, 3452796.870552968, 3452796.403340308, 3452796.3752547265, 3452796.2037282563, 3452796.0248268186, 3452795.8290828113, 3452795.802518943, 3452795.6734956712, 3452795.4888434033, 3452795.38760076, 3452795.3074917775, 3452795.1220016493, 3452795.059586271, 3452794.870385479, 3452794.772795678, 3452794.6688250834, 3452794.6000469583, 3452794.4997020387, 3452794.3381582284, 3452794.2620108044]
    ped_vx = [0.7238256335258484, 0.639672577381134, 0.7897564172744751, 0.6585690975189209, 0.6791093945503235, 0.7945690751075745, 0.6016510725021362, 0.460549920797348, 0.7457672953605652, 0.8732098340988159, 0.805212676525116, 1.0519752502441406, 1.1709372997283936, 1.1090607643127441, 1.060754418373108, 1.2517929077148438, 1.2421702146530151, 1.2008384466171265, 1.060544490814209, 1.0827932357788086, 0.9608871936798096, 0.9979074001312256, 1.2702217102050781, 1.262911319732666, 1.108064889907837, 1.178846001625061, 1.1997307538986206, 0.8758773803710938, 1.1190592050552368, 1.0902714729309082, 1.0724226236343384, 0.9496291875839233, 0.8632647395133972, 0.8452659845352173, 0.9867776036262512, 0.847038984298706, 0.7623805999755859, 0.8793231844902039, 0.9325444102287292, 0.7495713829994202, 0.5435964465141296, 0.28813278675079346, 0.46497365832328796, 0.5219458937644958, 0.444953590631485, 0.3514465093612671, 0.5726141333580017, 0.668704628944397, 0.4978678226470947, 0.37889906764030457, 0.5812103152275085, 0.6595303416252136, 0.39587685465812683, 0.41512081027030945, 0.473829448223114, 0.4206877052783966, 0.5919029116630554, 0.5470722317695618, 0.5153967142105103, 0.2822650671005249, 0.42101576924324036, 0.19177837669849396, 0.26111966371536255, 0.5238641500473022, 0.3501468300819397, 0.2952496111392975, 0.4474983811378479, 0.451397567987442, 0.5671170949935913, 0.5188154578208923, 0.4692383408546448, 0.2568801939487457, 0.41573265194892883]
    ped_vy = [-0.2130500227212906, -0.10842117667198181, -0.30901020765304565, -0.3418917655944824, -0.3550146222114563, -0.11495163291692734, -0.06626948714256287, -0.115512914955616, -0.1414618343114853, -0.13042031228542328, -0.05618571490049362, -0.233009934425354, -0.3721402883529663, -0.5309042930603027, -0.5551295280456543, -0.4548669755458832, -0.4668411910533905, -0.2531614303588867, -0.5055666565895081, -0.372606098651886, -0.3980276882648468, -0.5646659135818481, -0.49040424823760986, -0.3770105838775635, -0.30570921301841736, -0.38546639680862427, -0.38920164108276367, -0.17316950857639313, -0.17036254703998566, -0.1049417108297348, -0.17095723748207092, -0.16204555332660675, -0.3541968762874603, -0.23288148641586304, -0.17742377519607544, -0.19434435665607452, -0.49813875555992126, -0.5490744113922119, -0.6394816637039185, -0.554042637348175, -0.4390913248062134, -0.5146337151527405, -0.5495248436927795, -0.6552216410636902, -0.511761486530304, -0.7948772311210632, -1.0132256746292114, -0.9733282923698425, -1.231073021888733, -1.2844057083129883, -1.1654324531555176, -1.0570080280303955, -1.0976078510284424, -1.1070820093154907, -1.016290545463562, -0.9809934496879578, -1.1362088918685913, -1.1553255319595337, -1.0698350667953491, -1.0589735507965088, -1.049293041229248, -1.2025378942489624, -1.163025975227356, -1.1667100191116333, -1.1997871398925781, -1.1509995460510254, -1.3032857179641724, -1.417669415473938, -1.3370370864868164, -1.0651133060455322, -1.2295397520065308, -1.227583408355713, -1.1007834672927856]

    z = []
    for i in range(len(ped_x)):
        z.append(np.array([
            [ped_x[i]],
            [ped_vx[i]],
            [ped_y[i]],
            [ped_vy[i]]
        ]))
    return z

def veh_z_mia():
    veh_x = [
        739.6794456696546,
        739.9457292620912,
        739.9390335152374,
        739.9262512400034,
        739.9059787453987,
        739.8854519051506,
        739.8616394582762,
        739.8323979021022,
        739.7988022114209,
        739.7681812487398,
        739.7384492831201,
        739.7134731237061,
        739.6885867851354,
        739.6831673289680,
        739.6893052166449,
        739.7117539878232,
        739.7510500286336,
        739.7945538472072,
        739.8538032558058,
        739.9137715889711,
        739.9951309824976,
        740.1034856187266,
        740.2331660011013,
        740.3899532264899,
        740.6034654621083,
        740.8611719909883,
        741.1812109523418,
        741.5705503125356,
        742.0355360707657,
        742.6471978477078,
        743.3097248918915,
        744.0453259763724,
        745.1283810209459,
        746.1910210044331,
        747.3280391816162,
        748.5089097884813,
        749.5276712103077,
        750.5613287020236,
        751.5396167049110,
        753.1778337785466,
        754.8195191068477,
        756.1960848881993,
        757.2464030255368,
        758.1561682102274,
        759.1341189722203,
        759.9798316151705,
        761.0127317803251,
        761.9193801610568,
        762.9484504337926,
        763.8495637255278
    ]
    veh_y = [
        1688.9775850803603,
        1688.502671954344,
        1688.3736984101072,
        1688.0939365611425,
        1687.6392271086904,
        1687.215245523514,
        1686.7332487565257,
        1686.182475686139,
        1685.5442299991919,
        1684.9293234172674,
        1684.2610276402552,
        1683.5835787678448,
        1682.5848716283524,
        1681.8652548863206,
        1681.0339771277613,
        1680.2046813566672,
        1679.442714060014,
        1678.8681862990472,
        1678.2507163979017,
        1677.7550734619297,
        1677.170110061223,
        1676.5446207372104,
        1675.9398548033364,
        1675.3341920378248,
        1674.648983451885,
        1673.9753995307215,
        1673.2867375188507,
        1672.5847223863238,
        1671.8815710840165,
        1671.108791026441,
        1670.4124533557556,
        1669.762691094655,
        1668.9921606743626,
        1668.3871411347961,
        1667.8708248677601,
        1667.442543197128,
        1667.1527626935724,
        1666.9271758165385,
        1666.7686159324471,
        1666.5869599122964,
        1666.5038680838675,
        1666.4945779405343,
        1666.50894768548,
        1666.5346854523486,
        1666.56946186355,
        1666.610881860173,
        1666.66938327427,
        1666.7277046305953,
        1666.7994767763266,
        1666.8633091165948
    ]
    
    veh_vx = [0]
    for i in range(1, len(veh_x)):
        veh_vx.append((veh_x[i] - veh_x[i-1])/0.1)
    veh_vx[0] = veh_vx[1]

    veh_vy = [0]
    for i in range(1, len(veh_y)):
        veh_vy.append((veh_y[i] - veh_y[i-1])/0.1)
    veh_vy[0] = veh_vy[1]

    z = []
    for i in range(len(veh_x)):
        z.append(np.array([
            [veh_x[i]],
            [veh_vx[i]],
            [veh_y[i]],
            [veh_vy[i]]
        ]))
    return z
